#include "PWM.h"
#include "sys.h" 
#include "led.h"
#include "PID.h"
#include "math.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 
#include "usart.h"	 
#include "delay.h"
#define POWER_ADD  30
#include "usart.h"	  
//roll方向上的pid
 float volatile rol_p = 8,rol_i = 0.25,rol_d = 790;
 //pitch方向上的pid
 float volatile pit_p = 8,pit_i = 0.25,pit_d = 790;

  float R=10;                   //设置画直线的半径
  float step;					//测试阶跃相应变量

volatile  float	Rol_LastError; 				//Error[-1]
volatile  float Rol_PrevError;  			//Error[-2]
volatile  float Rol_SumError;
volatile  float Rol_Set_Pos;				 //Rol方向设定值
volatile  float Rol_Cur_Pos;				 //Rol方向当前值
	
	
volatile  float	Pit_LastError; 				//Error[-1]
volatile  float Pit_PrevError;  			//Error[-2]
volatile  float Pit_SumError;
volatile  float Pit_Set_Pos;				  //Pit方向设定值
volatile  float Pit_Cur_Pos;					//Pit方向当前值
	
extern	u16 coordinate_buffer[10];  
	
	u16 time=0;		         //工作时间，5ms加一次
  u8 mode_freedom_flage=0;
volatile  float	mode_freedom_X=0;
volatile  float	mode_freedom_Y=0;

//定时器3中断服务函数
void TIM3_IRQHandler(void)
{
	if(TIM3->SR&0x0001)	//overflow interrupt
	{
		LED1 = !LED1;
	}
	TIM3->SR &= 0xfffe;	//clear flag of the interrupt
}
//general timer initial

void TIM3_Int_Init(u16 arr,u16 psc)
{
	RCC->APB1ENR |= 1<<1;			//Timer3 enable
	TIM3->ARR = arr;					//Set the reload value
	TIM3->PSC = psc;
	TIM3->DIER |= 1<<0;
	TIM3->CR1 |= 0x01;
	MY_NVIC_Init(1,2,TIM3_IRQn,2);
}

//定时器3输出PWM，初始化四路输出
void TIM3_PWM_Init(u16 arr,u16 psc)
{
	RCC->APB1ENR |= 1<<1;			//Timer3 clock enable
	RCC->APB2ENR |= 3<<2;			//Enable the clock of PORTB and PORTA

	// PA6,7 输出配置 (对应 TIM3_CH1 & TIM3_CH2)
	GPIOA->MODER   &= ~(GPIO_MODER_MODER6 | GPIO_MODER_MODER7);  // 清除PA6、PA7模式
	GPIOA->MODER   |=  (GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1);  // 设为复用功能模式
	GPIOA->OTYPER  &= ~(GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7);   // 推挽输出
	GPIOA->OSPEEDR |=  (GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7);  // 高速模式
	GPIOA->PUPDR   &= ~(GPIO_PUPDR_PUPDR6 | GPIO_PUPDR_PUPDR7);  // 无上下拉
	GPIOA->AFR[0]  &= ~(0xFF000000);  // 清除PA6、PA7复用功能
	GPIOA->AFR[0]  |=  (0x22000000);  // 配置为AF2(TIM3)
	
	/*
	GPIO_Set(GPIOA,PIN6|PIN7,GPIO_MODE_AF,GPIO_OTYPE_PP,GPIO_SPEED_100M,GPIO_PUPD_NONE);
	GPIO_AF_Set(GPIOA,6,2);	//PA6,AF2(TIM3)
	GPIO_AF_Set(GPIOA,7,2);	//PA7,AF2(TIM3)
	*/
	
	// PB0,1(TIM3_CH3 & TIM3_CH4)
	GPIOB->MODER   &= ~(GPIO_MODER_MODER0 | GPIO_MODER_MODER1);
	GPIOB->MODER   |=  (GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1);
	GPIOB->OTYPER  &= ~(GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1);
	GPIOB->OSPEEDR |=  (GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1);
	GPIOB->PUPDR   &= ~(GPIO_PUPDR_PUPDR0 | GPIO_PUPDR_PUPDR1);
	GPIOB->AFR[0]  &= ~(0x000000FF);
	GPIOB->AFR[0]  |=  (0x00000022);
	
	TIM3->ARR = arr;
	TIM3->PSC = psc;
	
	TIM3->CCMR1 &= 0x8787;
	TIM3->CCMR1 |= 0x7878;		//配置CH1,CH2 PWM模式，设置CH1,CH2的预装载值
	TIM3->CCMR2 &= 0x8787;
	TIM3->CCMR2 |= 0x7878;		//配置CH3,CH4 PWM模式，设置CH3,CH4的预装载值
	
	TIM3->CCER &= 0xEEEE;
	TIM3->CCER |= 0x1111;
	
	TIM3->CR1 = 0x0080;				//ARPE enable
	TIM3->CR1 |= 0x01;				//timer3 enable
	
		
	TIM3->CCR1 = 1000;				//设定CH1占空比（实际上占空比取反，占空比为60%）
	TIM3->CCR2 = 1000;				//设定CH2占空比（实际上占空比取反，占空比为60%）
	TIM3->CCR3 = 1000;				//设定CH3占空比（实际上占空比取反，占空比为60%）
	TIM3->CCR4 = 1000;				//设定CH4占空比（实际上占空比取反，占空比为60%）
}


//TIMER2初始化函数
void TIM2_Int_Init(u16 arr,u16 psc)
{
	RCC->APB1ENR |= 1<<0;			//Timer2 clock enable
		
	TIM2->ARR = arr;					//Timer2 重装载值
	TIM2->PSC = psc;					//Timer2 预分频值
	
	TIM2->DIER |= 1<<0;				//定时器更新中断
	TIM2->CR1 |= 0x0080;			//ARPE enable自动装载
	TIM2->CR1 |= 0x01;				//timer3 enable
	MY_NVIC_Init(1,3,TIM2_IRQn,2);
}

/***************************************************模式选择*****************************************************************************/



//Mode1 画直线
void mode_1(void)
{
	float A = 0.0;						//角度
	short rol_pwm,pit_pwm;
	const float priod = 1560;//1560.0;  				//单摆周期(毫秒)	
	rol_p = 40;rol_i = 0;rol_d =3200;// 8   0.25  790   
	pit_p = 50; pit_i = 0.3;pit_d =3200;	//mode1的工作参数
	time+=5;		//计时加5ms
	if(time>=priod) time=0;
	   A =9.4662;                   //根据摆幅求出角度A,62.5为摆杆距离地面长度cm
	Rol_Set_Pos =A*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
  Pit_Set_Pos = 0;	//Pit方向上电机控制

	
    //	printf ("%f\n",roll);
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
  		Rol_Mot_Con(rol_pwm);
	
	
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
		 Pit_Mot_Con(pit_pwm);
	//printf ("A:%f B%f\n",Rol_Set_Pos,roll);
				
}

//方式二，设定45°直线
void mode_2(void)
{

	float A = 0.0;						//
	short  rol_pwm,pit_pwm;
	const float priod = 1560;  				//单摆周期(毫秒)	
//	rol_p = 30;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 30;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
	rol_p = 38;rol_i = 0;rol_d =4520;// 8   0.25  790   
	pit_p = 38;pit_i = 0;pit_d =4520;	//mode1的工作参数
	time+=5;														//计时加5ms
	if(time>=priod) time=0; 
	A =12; //14;//atan((0.3/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	Rol_Set_Pos = A*sin(6.283184*time/priod);	//Rol方向上电机控制	
	Pit_Set_Pos = Rol_Set_Pos ;	//Pit方向上电机控制
	 			
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
			Rol_Mot_Con(rol_pwm);
											
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);
	
}

//30°直线
void mode_a(void)
{

	float A = 0.0;						//
	short rol_pwm,pit_pwm;
	const float priod = 1560;  				//单摆周期(毫秒)	
//	rol_p = 30;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 30;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
	rol_p = 40;rol_i = 0;rol_d =4520;// 8   0.25  790   
	pit_p = 40;pit_i = 0;pit_d =4520;	//mode1的工作参数
	time+=5;	//计时加5ms
	if(time>=priod) time=0;
	A = 9.5;//atan((0.3/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	Rol_Set_Pos = A*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
	Pit_Set_Pos = 0.57735*A*sin(6.283184*time/priod);	//Pit方向上电机控制
	 			
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
			Rol_Mot_Con(rol_pwm);
											
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);
			  TIM3->CCR1 -=20;
				TIM3->CCR2 -=20;
	
}

//60°直线
void mode_b(void)
{

	float A = 0.0;						//
	short rol_pwm,pit_pwm;
	const float priod = 1560;  				//单摆周期(毫秒)	
//	rol_p = 30;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 30;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
	rol_p = 38;rol_i = 0;rol_d =4520;// 8   0.25  790   
	pit_p = 38;pit_i = 0;pit_d =4520;	//mode1的工作参数
	time+=5;														//计时加5ms
	if(time>=priod) time=0;
	A = 6;//atan((0.3/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	Rol_Set_Pos = A*sin(6.283184*time/priod);	//Rol方向上电机控制	
	Pit_Set_Pos = 1.73205080*A*sin(6.283184*time/priod);	//Pit方向上电机控制
	 			
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
			Rol_Mot_Con(rol_pwm);
											
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);

}

//方式三画小圆
void mode_3(void)
{
	
	float A = 0.0;						//
	short rol_pwm,pit_pwm;
	const float priod = 1560.0;  				//单摆周期(毫秒)	
//	rol_p = 30;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 30;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
	rol_p = 40;rol_i = 0;rol_d =3200;// 8   0.25  790   
	pit_p = 45;pit_i = 0;pit_d =3800;	//mode1的工作参数
	time+=5;														//计时加5ms
	A = 5;//atan((R/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	Rol_Set_Pos = A*sin(6.283184*time/priod);	//Rol方向上电机控制	
	Pit_Set_Pos = A*sin(6.283184*time/priod+3.14159/2);	//Pit方向上电机控制
	
			
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
//	    rol_pwm -= 20;
			Rol_Mot_Con(rol_pwm);

											
	 		Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			if(pit_pwm >= 0)
			pit_pwm+=180;
			else
			pit_pwm-=180;
			Pit_Mot_Con(pit_pwm);
}
//画中圆
void mode_4(void)
{
	
	float A = 0.0;						//
	short rol_pwm,pit_pwm;
	const float priod = 1560.0;  				//单摆周期(毫秒)	
//	rol_p = 26;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 26;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
	   rol_p = 40;rol_i = 0;rol_d =3200;// 8   0.25  790   
     pit_p = 43;pit_i = 0;pit_d =3400;	//mode1的工作参数
   	 time+=5;														//计时加5ms
	   A = 7.3;//atan((R/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	   Rol_Set_Pos = A*sin(6.283184*time/priod);	//Rol方向上电机控制	
	   Pit_Set_Pos = A*sin(6.283184*time/priod+1.5707963);	//Pit方向上电机控制
	
			
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
//	    rol_pwm -= 20;
			Rol_Mot_Con(rol_pwm);

											
	 		Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			if(pit_pwm >= 0)
			pit_pwm+=180;
			else
			pit_pwm-=180;
			Pit_Mot_Con(pit_pwm);
			
}
//画90°直线
void mode_5(void)
{
	
	float A = 0.0;						//角度
	short rol_pwm,pit_pwm;
	const float priod = 1560;//1560.0;  				//单摆周期(毫秒)	
	rol_p = 50;rol_i = 0;rol_d =3200;// 8   0.25  790   
	pit_p = 40; pit_i = 0.3;pit_d =3200;	//mode1的工作参数
	time+=5;		//计时加5ms
	if(time>=priod) time=0;
	   A =9;                   //根据摆幅求出角度A,62.5为摆杆距离地面长度cm
	Rol_Set_Pos = 0;	//Rol方向上电机控制	
  Pit_Set_Pos = A*sin(6.283184*time/priod);	//Pit方向上电机控制

    //	printf ("%f\n",roll);
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
  		Rol_Mot_Con(rol_pwm);
	
	
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
		 Pit_Mot_Con(pit_pwm);
			
}

//画图形
void mode_7(void)
{
	
	float A = 0.0;						
	short rol_pwm,pit_pwm;
	const float priod = 3120.0;  				//单摆周期(毫秒)	
//	rol_p = 26;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 26;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
		rol_p = 30;rol_i = 0;rol_d =1900;// 8   0.25  790   
	pit_p = 30;pit_i = 0;pit_d =1900;	//mode1的工作参数
	time+=5;														//计时加5ms
	A = 10;//atan((R/0.87))*57.2958f;				 //根据摆幅求出角度A,0.87为摆杆距离地面长度cm
	if(time<=780)
	{
		Rol_Set_Pos = A;
		Pit_Set_Pos = A*cos(6.283184*time/priod);
	}
	else if(time<=1560)
	{
		Rol_Set_Pos = -A*cos(6.283184*time/priod);
		Pit_Set_Pos = -A;
	} 
	else if(time<=2340)
	{
		Rol_Set_Pos = -A;
		Pit_Set_Pos = -A*cos(6.283184*time/priod);
	}
	else if(time<=3120)
	{
		Rol_Set_Pos = A*cos(6.283184*time/priod);
		Pit_Set_Pos = A;
	}
	else
	{
		time = 5;
	}


			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
			Rol_Mot_Con(rol_pwm);
											
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);
		
}



//Mode8，正方形
void mode_8(void)
{
	float A = 0.0;						//角度
	short rol_pwm,pit_pwm;
	const float priod = 3120*4 ;//1560.0;  				//单摆周期(毫秒)	
//	rol_p = 26;rol_i = 0.25;rol_d =1300;// 8   0.25  790   
//	pit_p = 26;pit_i = 0.25;pit_d =1300;	//mode1的工作参数
		rol_p = 30;rol_i = 0;rol_d =1900;// 8   0.25  790   
	pit_p = 30;pit_i = 0;pit_d =1900;	//mode1的工作参数
	time+=5;		//计时加5ms
	if(time>=2*priod) time=0;
	A = 3;//atan((R/62.5))*57.2958f;				 //根据摆幅求出角度A,62.5为摆杆距离地面长度cm
	if(time<=(priod/2))
	{
	 Rol_Set_Pos =A;//*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
   Pit_Set_Pos =A*cos(2*3.14159*time/2*priod);	//Pit方向上电机控制
	}
	if(time>(priod/2) && time<=priod)
	{
	 Rol_Set_Pos =A*cos(2*3.14159*time/2*priod);//*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
   Pit_Set_Pos =-A;	//Pit方向上电机控制
	}	
	if((time>priod) && (time<=priod*3/2))
	{
	 Rol_Set_Pos =-A;//*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
   Pit_Set_Pos =-A*cos(2*3.14159*time/2*priod);	//Pit方向上电机控制
	}	
	if(time>(priod*3/2) && time<=priod*2)
	{
	 Rol_Set_Pos =-A*cos(2*3.14159*time/2*priod);//*sin(2*3.14159*time/priod);	//Rol方向上电机控制	
   Pit_Set_Pos =A;	//Pit方向上电机控制
	}	
	
			Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
			Rol_Mot_Con(rol_pwm);
											
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);
}    
	//Mode9归位
void mode_9(void)
{
	short rol_pwm,pit_pwm;
	rol_p = 40;rol_i = 0.2;rol_d =4800;// 8   0.25  790   
	pit_p = 40;pit_i = 0.2;pit_d =4800;	//mode1的工作参数
	Rol_Set_Pos =0;	//Rol方向上电机控制	
  Pit_Set_Pos =0;	//Pit方向上电机控制

			Rol_Cur_Pos = roll;	//Rol方向控制
			Pit_Cur_Pos = pitch;	//Pit方向控制
	
	
	   if(Rol_Cur_Pos>=5.5) 
			rol_pwm =800;//PID_Rol_PosCalc(Rol_Cur_Pos);
		 else
			 {
	      if(Rol_Cur_Pos<=-5.5) 
	  	     rol_pwm =-800;
				else
					rol_pwm =PID_Rol_PosCalc(Rol_Cur_Pos);
	  	}
 	  	Rol_Mot_Con(rol_pwm);
	
	
	   if(Pit_Cur_Pos>=5.5) 
			pit_pwm =800;//PID_Rol_PosCalc(Rol_Cur_Pos);
		 else
			 {
	      if(Pit_Cur_Pos<=-5.5) 
	  	     pit_pwm =-800;
				else
					pit_pwm =PID_Pit_PosCalc(Pit_Cur_Pos);
	  	}
			Pit_Mot_Con(pit_pwm);

}


void  STOP(void)
{
  TIM3->CCR1=1000;
	TIM3->CCR2=1000;
	TIM3->CCR3=1000;
	TIM3->CCR4=1000;
}



void mode_fredom(void)
{

	
	short rol_pwm,pit_pwm;
	  
	
	                                                                                         
	rol_p = 40;rol_i = 0.2;rol_d =4520;// 8   0.25  790   
	pit_p = 40;pit_i = 0.2;pit_d =4520;	//mode1的工作参数
	
	 mode_freedom_X = -((coordinate_buffer[1 ]*256+coordinate_buffer[2])-160);
	 mode_freedom_Y = coordinate_buffer[4]-120;

	
	Rol_Set_Pos =mode_freedom_X/5.5;
  Pit_Set_Pos =mode_freedom_Y/5.5;
		Rol_Cur_Pos = roll;									//Rol方向控制
			rol_pwm = PID_Rol_PosCalc(Rol_Cur_Pos);
  		Rol_Mot_Con(rol_pwm);
	
	
			Pit_Cur_Pos = pitch;								//Pit方向控制
			pit_pwm = PID_Pit_PosCalc(Pit_Cur_Pos);
			Pit_Mot_Con(pit_pwm);
			LED1=!LED1;

}

//Timer2 中断服务函数
void TIM2_IRQHandler(void)
{				
	
	TIM2->SR &= ~(1<<0);			//清除中断标志位
	if(mode_freedom_flage == 0)
	{
		
	switch(USART_RX_BUF[0])//根据题目选择函数
		{	
			case 1: mode_1(); break;
			case 2: mode_2(); break;
			case 3: mode_3(); break;
			case 4: mode_4(); break;
			case 5: mode_5(); break;
			
			case 6: STOP(); break;
			case 7: mode_7(); break;
			case 8: mode_8(); break;
			case 9: mode_9(); break;
		  case 0x0a: mode_a();break;
			case 0x0b: mode_b();break;
		  case 0x11: STOP();mode_freedom_flage=1;coordinate_buffer[1]=0;coordinate_buffer[2]=160;coordinate_buffer[4]=120;break;
			case 0xee: STOP(); break;
			default:break;		
		}
	}
	else //自由模式
		{
			mode_fredom();
		}
		
			
}

